在学习Moveit相关教程后开始尝试动手搭建双臂机器人仿真平台，整个实现过程中多次碰壁，考虑当前网络上关于双臂机器人的仿真教程解答相对较少并不乏谬误，于是将实现流程、注意事项及个人总结分享在网络，本文不做过多教程类解释（如有需要可参考Moveit官方教程或其他博主资源），仅以记录形式方便大家和未来的自己参考。

文中代码多仅呈现关键部分，未做完整展示，项目完整代码可参考 https://github.com/Qiaozs/Moveit-Gazebo-for-yumi-ABB。

部分图片资源及参考资料来自网络，如有侵权，请联系删除。

ubuntu20.04 ROS-noetic 提前安装好ROS对应版本下的Moveit和Gazebo

本文选用ABB公司yumi双臂机器人，一方面因为就读院校拥有一台yumi实机，也是实验室为数不多的双臂机器人，故对其相对熟悉；另一方面ABB公司毕竟为机器人大厂，网络开源资料相对充足，且yumi确是比较知名的双臂机器人，方便在实现的过程中参考网络资料。（图片来自网络）

基于基本仿真平台实现简单的运动规划执行（定点运动、绕八字等）； 基于双臂协同仿真优化运动实现效果； 另外由于npy喜欢玩蛋仔派对，遂也考虑使用yumi机器人模仿蛋仔游戏中一段人物舞蹈，比较契合机器人的双臂特点，实现难度适中，对自己也有不错的锻炼效果。

本文计划完整记录整个实现流程，故整体分为两个部分：基本仿真实现和双臂协同仿真实现。前者方便读者用来作简单机械臂仿真参考，后者具体实现双臂协同目标。主要步骤包括如下：

基本仿真实现

双臂协同仿真实现

本部分基本实现 yumi 机器人在 rviz 和 gazebo 中的通讯建立，最终可以通过moveit的运动规划来实现机器人在 gazebo 中做简单动作（如走直线、绕八字）。建议读者在学习 Moveit 教程后（或学习过程中）参考本文，效果更佳。

开始之前创建 yumi_ws 工作空间

ROS 中描述机器人模型通常使用 urdf 或 xacro 文件，两者均是标记语言，其中 urdf 相对原始， 而 xacro 对 urdf 作了一定优化，使用宏定义等方式有效减少了 urdf 文件代码的冗余性，实际使用中对二者的选择限制并不明显，本文中更多使用前者。 关于 urdf 文件，一般有以下几种方式获得：

对于使用现有的产品级机器人建议直接从网络上下载相关机器人资源，本文使用的 yumi 机器人在可从ABB官方开源的 github 项目中下载，也可从网友上传的资源中下载。 需要的资源包括如下： 其中 urdf 文件夹下存放 yumi.urdf 文件（模型文件），meshe 文件夹中为 yumi 机器人的贴图文件（会在 yumi.urdf 中被调用） 在工作空间中创建 yumi_description 功能包（标准命名 robot_description，添加 urdf 、xacro依赖，添加上述相关文件即可。 网络常见的资源通常也为 yumi_description 功能包形式，一般可以直接下载使用。 此处可以 cd 到 yumi.urdf 所在目录下输入：

对机器人模型作简单检查，正常情况终端显示： 也可以使用

在当前目录下生成pdf文件用来直观认识机器人模型结构，检查无误后本步完成。

注意：网络上的机器人模型文件众多，但有不少存在细微错误无法用于后续的Moveit+Gazebo通信建立并且较难发觉（在后文中仍会提到，此处仅作提醒），不一定要使用ABB官方提供的文件，有时经过网友实践检验的反而更佳。

打开终端运行

启动 Moveit 配置工具，按照流程完成Moveit配置：

正常情况会启动Riz仿真环境，检查是否可以对两个 group 分别做运动 plan 和 execcute： 在 planning Group 处切换规划组。检查无误，Moveit 配置完成。

参考网络教程，此部分需要完成多个文件的修改、添加，由于网络资料介绍、ROS版本等参差不一，整个实现中绝大多数报错来自此部分，需要读者格外注意。

准备工作：src 下新建 yumi_gazebo 功能包，包含 参数配置config 文件夹和 启动文件 launch 文件夹。

终端运行：

启动gazebo仿真环境，正常情况为机器人在gazebo中正常显示，终端没有报错。 注意：常见的终端报错内容有：

机器人link质量或转动惯量设定有误，即也是urdf文件问题。

解决方法：删除urdf文件中的中文内容，或者前往主目录下/usr/lib/python2.7/xml/etree/ElementTree.py修改不识别中文的编码问题，可参考此处，但后者相对麻烦，建议修改前者。

其余还可能遇到各种错误，网络上的报错解决方案也五花八门，修改起来费时费力，但作者的经验是：当遇到报错且存在上述第一条报错时，优先解决这一条，其他的有可能会随之解决。

从此处开始，以下全部文件均默认自行新建

参数配置文件：yumi_trajectory_control.yaml

着重注意：

控制器启动文件yumi_trajectory_controller.launch

注意点：

参数配置文件：yumi_joint_states.yaml

控制器启动文件yumi_gazebo_states.launch

有些教程中将关节状态控制器和关节轨迹控制器写在一起，同样也可以实现期望效果。

参数配置文件：controllers_gazebo.yaml

name处写作yumi/arm_l_controller,即命名空间/运动规划组控制器，有些教程中不强调写命名空间即，只写作arm_l_controller，实际中也是有无均可，但可以给自己起到提醒作用。 控制器启动文件yumi_moveit_controller_manager.launch.xml

moveit_planning_execution.launch

本文件用于一次性启动上述的各个文件:yumi_bringup_moveit.launch

从上到下依次启动了四个launch文件。

以上流程为参照相关教程实现，但是参考教程实现过程中可能会遇到如下问题：

至此成功实现Moveit+Gazebo的联合仿真通讯建立，下一步可以通过编写运动规划代码来交给Moveit做运动规划，并在Gazebo中呈现。

Moveit做运动规划的好处在于其提供了运动学求解工具，并且会自动进行碰撞检测。使用Moveit做运动规划一般有三种方式：Rviz界面拖动规划、Python编程控制、C++编程控制。由于Python代码相对简洁易懂，所以本文均使用Python作为编程语言。 在基本仿真中主要实现了定点运动、绕八字运动和椭圆运动。全部代码见开源文档，此处只做部分展示。

实现定点运动有两种方案：1、设定关节角度；2、设定末端位姿 此处对设定末端姿态方式做如下展示： 末端姿态使用xyz坐标和四元数设定，此处为了避免设定多位置带来的代码冗余，定义一姿态设定函数，arm.go()等运动命令封装在move()函数中：

随后标定目标位置执行：

实际操作中，机械臂末端四元数的设置相对麻烦，需要频繁得计算确定，此处给出一种相对简单方案： 可以在Rviz界面中拖动机械臂调整末端位姿，在python代码中调用move_group.get_current_pose() 函数用来提取当前机械臂位姿信息，实现方式如下（可见于get_pose.py文件)

终端输出的位姿信息可以直接用于设定期望末端位姿。 设定若干目标位姿后可实现如下效果：

注意：可以看到代码中分左右臂两部分编写，实际运行中，yumi机器人双臂也为先后运行，即无法做到同步运动，这是因为Moveit没有办法同时规划两个运动，即便使用两个终端同时运行两个脚本文件也无法实现（亲测，强行同时规划两个运动时，机器人双臂出现争夺现象且均无法正常运动），如此仿真完全无法发挥yumi双臂机器人优势，于是本文在下一章节给出双臂同时规划的方案。

区别于定点运动，八字运动强调轨迹的连续性、确定性，所以使用笛卡尔空间轨迹规划法，沿期望轨迹连续采点，plan出整体的轨迹随后execute。此处为了实现圆周运动，使用简单的三角函数原理，同样若要画椭圆运动，对三角函数系数稍作修改即可。 现定义circle_move_yz函数用来实现机械臂圆周运动：